﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Microsoft.Robotics.Services.DepthCamSensor;

namespace NestedParticleFilter
{
    /// <summary>
    /// Represents Particle filtering algorithm, propagation and observation, normalization and resampling
    /// </summary>
    public class ParticleFilter
    {
        #region constants

        public static double UpNegativeObsThreshold = 2; // upper bound negative observation threshold in m
        public static double LowNegativeObsThreshold =0.5; // lower bound negative observation Threshold in m
        private static int _count =0;
        private static double JPushVelocity = 300.0; //mm/sec
        //private static int JPushTime = 7; //sec
        public static bool next;

        #endregion 

        #region Motion Noise and Observation noise Constants
        //noise parameters
        const double Alpha1 = 0.01; // motion noises
        const double Alpha2 = 0.01; // motion noises
        const double Alpha3 = 0.3; // motion noises
        const double Alpha4 = 0.3; // motion noises
        const double Alpha5 = 0.5; // motion noises 1.6
        const double Alpha6 = 0.5; // motion noises 1.6
        
        const double VarR = 0.05;
        const double VarFi = 0.1; //1.5
        const double VarS = 100;

        private const double JStraightProbability = 0.9;
        private const double JLeftProbability = 0.05;
        private const double JRightProbability = 0.05;

        private const double JActionTime = 2.1; // seconds
 
        #endregion

        private static Random _rand;
       
        #region Motion Model Velocity
        /// <summary>
        /// Calculates the probability of the robot's pose at time t, based on robot's pose at time t-1, and motion model
        /// </summary>
        /// <param name="model">Motion Model</param>
        /// <param name="pose">Robot's pose at time t-1</param>
        /// <param name="hypothesizedPose">Hypothesized robot's pose at time t</param>
        /// <param name="time">The time that takes for the robot to move from point x to point x' (sec)</param>
        /// <returns>Probability that the robot is at point x'</returns>
        private static double MotionModelVelocity(Motions model, RobotPose pose, RobotPose hypothesizedPose, double time)
        {
            double mu;
            double v = model.Velocity;  // meters per sec
            double w = UnitConversion.DegreeToRadian(model.Omega);       // radiant per sec
            //Console.WriteLine("\n\tComanded Speed: " +v +"\n\tComanded Rotational Velocity: " + w);

            /* degree to radian for furture calculation */
            double theta = UnitConversion.DegreeToRadian(pose.Theta);  // converting the theta at time t-1 to radian.
            double htheta = UnitConversion.DegreeToRadian(hypothesizedPose.Theta); // converting the hypothesized theta to radian.

            //double numerator = (pose.z - hypothesizedPose.z) * (-Math.Cos(theta)) + (pose.x - hypothesizedPose.x) * (-Math.Sin(theta));
            double numerator = (-pose.Z + hypothesizedPose.Z) * (Math.Cos(theta)) + (-pose.X + hypothesizedPose.X) * (Math.Sin(theta));
            //double denominator = (pose.x - hypothesizedPose.x) * (-Math.Cos(theta)) - (pose.z - hypothesizedPose.z) * (-Math.Sin(theta));
            double denominator = (-pose.X + hypothesizedPose.X) * (Math.Cos(theta)) - (-pose.Z + hypothesizedPose.Z) * (Math.Sin(theta));
            if (denominator == 0)
                mu = 0.5 * (numerator / 0.00001);
            else
                mu = 0.5 * (numerator / denominator);

            // Console.WriteLine( pose.x +" " + hypothesizedPose.x+ " "+ Math.Cos(theta) + " "+ pose.z +" "+hypothesizedPose.z  + " " +Math.Sin(theta));
            // Console.WriteLine("\t mu is: " + mu);

            // calculate the position of the center of the circle. 
            double cz = (pose.Z + hypothesizedPose.Z) * 0.5 + mu * (pose.X - hypothesizedPose.X);
            double cx = (pose.X + hypothesizedPose.X) * 0.5 + mu * (hypothesizedPose.Z - pose.Z);
            double radius = Math.Sqrt(Math.Pow(pose.Z - cz, 2) + Math.Pow(pose.X - cx, 2));

            //Console.WriteLine("\tCircle ({0}, {1}) , Radius: {2}", cx, cz, radius);

            /* change in the heading direction.*/
            /* In probabilistic robotic book, there isn't any negative sign in front of dtheta, and that is because the book assumes turning counter clockwise is a positive motion.
             * In Microsoft robotic however, turning counter clock wise should have a negative sign and clockwise turning is positive.
             */
            double dtheta = -(Math.Atan2(hypothesizedPose.X - cx, hypothesizedPose.Z - cz) - Math.Atan2(pose.X - cx, pose.Z - cz));    // delta theta in radian
            //Console.WriteLine("dtheta " + dtheta + " time : "+ time);

            /* v and w of a noise free model*/
            double newv = (dtheta / time) * radius;
            double neww = dtheta / time;


            double gama = (htheta - theta) / time - neww;

            //Console.WriteLine("\tdtheta: {0}, newv: {1}, neww: {2}, gama: {3}, time {4}", dtheta, newv, neww, gama, time);
            // Console.WriteLine("\tCalculated Velocity: {0} ", newv);

            /********************************************************************************************************
             *                                             calculate the probability
             ********************************************************************************************************/
            double err1 = (v - newv);
            // Console.WriteLine(" v: " + v + " new v : " + newv);
            double err2 = (w - neww);
            double err3 = (gama);
            double var1 = Alpha1 * Math.Pow(v, 2) + Alpha2 * Math.Pow(w, 2);   // variance for the first probability distribution
            double var2 = Alpha3 * Math.Pow(v, 2) + Alpha4 * Math.Pow(w, 2);  // variance for the second probability distribution
            double var3 = Alpha5 * Math.Pow(v, 2) + Alpha6 * Math.Pow(w, 2);   // variance for the third probability distribution

            // Console.WriteLine("Err1: " + err1 + "err2: "+err2+ " err3: "+err3+" Variance: " + var1);

            // calculate zero centered (mean =0 ) probability distribution
            //Console.WriteLine("Probability : " + Probability.prob_normal_distribution(err1, 0, var1) * Probability.prob_normal_distribution(err2, 0, var2) *
            //  Probability.prob_normal_distribution(gama, 0, var3));

            return Probability.ProbNormalDistribution(err1, 0, var1) * Probability.ProbNormalDistribution(err2, 0, var2) *
                Probability.ProbNormalDistribution(gama, 0, var3);

        }
        #endregion


        #region Sample Motion Model Velocity
        /// <summary>
        /// Samples a new pose from pose at time t-1
        /// </summary>
        /// <param name="motion">motion model with velocity and omega</param>
        /// <param name="pose">pose at time t-1</param>
        /// <param name="time">time it has traveled in second</param>
        /// <returns>New postion of the robot based on the action it took and time it took to do the action</returns>
        private static RobotPose SampleMotionModelVelocity(Motions motion, RobotPose pose, double time)
        {
            //motion noise
            //time -= timeEpsilon;
            double b1 = 0.05, b2 = 0.05, b3 = 0.001, b4 = 0.001, b5 = 0.005, b6 = 0.005;
            double v = motion.Velocity;
            
            double w = UnitConversion.DegreeToRadian(motion.Omega);                     // convert w to radian per sec.
            double theta = UnitConversion.DegreeToRadian(pose.Theta);
            double newv = v + Probability.SampleNormalDistribution(b1 * v * v + b2 * w * w);
            double neww = w + Probability.SampleNormalDistribution(b3 * v * v + b4 * w * w);
            double newgamma = Probability.SampleNormalDistribution(b5 * v * v + b6 * w * w);
            
            Debug.Assert(!Double.IsNaN(neww) || !Double.IsNaN(newv), "We cannot generate NaN \nnewv:" +
                newv + "\nneww: " + neww ); 
            
            double newz = pose.Z + (newv / neww) * Math.Sin(theta) - (newv / neww) * Math.Sin(UnitConversion.PutThetaInBound((theta + neww * time)));
            double newx = pose.X - (newv / neww) * Math.Cos(theta) + (newv / neww) * Math.Cos(UnitConversion.PutThetaInBound((theta + neww * time)));
            double newtheta = UnitConversion.PutThetaInBound(theta - neww * time - newgamma * time);  // in radian  see if this without  minus gives you the right new theta

            
            double t  = UnitConversion.RadianToDegree(newtheta);
            Debug.Assert(!Double.IsNaN(newz) || !Double.IsNaN(newx), "We cannot generate NaN \nMotion:" + 
                motion.ToString()+"\nPose: "+pose.ToString()+ "\nTime: "+ time +"\nnewv:" +
                newv + "\nneww: " + neww); 
            Debug.Assert(t <= 180 && t >= -180, "Theta out of range: before puting in boubd: " + (theta - neww * time - newgamma * time) + " after putting in bound:  " + newtheta + " (radian), " + t + "(Degree)");
            RobotPose newPose = new RobotPose(newx, newz, t);
            //Debug.WriteLine("New Pose: "+ newPose);
            return newPose;

        }
        #endregion

        #region Sample Motion Model Odometry 

       /// <summary>
       /// Samples new particle based on the odometry of the mobile robot
       /// </summary>
       /// <param name="prevPose">prev pose based on the odometry</param>
       /// <param name="particlePose"> particle pose </param>
       /// <param name="newPose"> new pose based on the odometry</param>
       /// <returns>sample new pose </returns>
        private static  RobotPose SampleMotionModelOdometry (RobotPose prevPose , RobotPose particlePose, RobotPose newPose)
        {
            double b1 = 0.05, b2 = 0.05, b3 = 0.0001, b4 = 0.0001;
            double prevTheta = UnitConversion.DegreeToRadian(prevPose.Theta); // in radian
            double newTheta = UnitConversion.DegreeToRadian(newPose.Theta); // in radian
            double theta = UnitConversion.DegreeToRadian(particlePose.Theta);
           
            double delta1 = UnitConversion.PutThetaInBound(Math.Atan2((prevPose.X - newPose.X),(prevPose.Z - newPose.Z)) - newTheta); // in radian
            double deltaDistance = Math.Sqrt(Math.Pow(prevPose.X - newPose.X, 2) + Math.Pow(prevPose.Z - newPose.Z, 2));
            double delta2 = UnitConversion.PutThetaInBound(newPose.Theta - prevPose.Theta - UnitConversion.RadianToDegree(delta1)); // in degree

            double newDelta1 = delta1 - Probability.SampleNormalDistribution(b1*Math.Abs(delta1) + b2*deltaDistance);
            double newDeltaDistance = deltaDistance -
                                      Probability.SampleNormalDistribution(b3*deltaDistance +
                                                                           b4*(Math.Abs(delta1) + Math.Abs(delta2)));
            double newDelta2 = delta2 - Probability.SampleNormalDistribution(b1*Math.Abs(delta2) + b2*deltaDistance);

            double newx = particlePose.X + newDeltaDistance*Math.Cos(theta + newDelta1);
            double newz = particlePose.Z + newDeltaDistance*Math.Sign(theta + newDelta2);
            double newtheta = theta + newDelta1 + newDelta2;
            newtheta = UnitConversion.PutThetaInBound(UnitConversion.RadianToDegree(newtheta));
            RobotPose pose = new RobotPose(newx,newz, UnitConversion.RadianToDegree(newtheta));
            return pose;
        }

        #endregion 

        #region Propagate

        /// <summary>
        /// Based on the motion  (either the motion is straight (based on distance) or in a circle fashion), this function will sample
        ///  m different poses at time t, for each particle at time t-1. 
        /// </summary>
        /// <param name="particles">set of particles representing the hypothetical location of the robot</param>
        /// <param name="parameter"> parameter has the motion, time and all the scenery information that was gathered from processing the image</param>
        /// <param name="jVelocity">Robot J's velocity in m/sec</param>
        /// <param name="mapLandmarks">the believed landmarks of the map</param>
        /// <param name="depth">depth of the observed landmark</param>
        /// <param name="angle">angle of observed landmark</param>
        /// <param name="color">color of observed labdnarj</param>
        /// <param name="level">level of nestedness of particles. (Level 1 indicates the top level, level 0 is the nested particles)</param>
        /// <param name="lastindex">last index of the particle inwhich it got propagated</param>
        //public static void Propagate(Motions motion, double iTime, List<Particle> particles,
        //    double jVelocity, List<Landmark> mapLandmarks, List<IdentifiedLandmark> identifiedLandmarks,
        //    bool observedRobot, double depth, double angle, CustomisedColor color, DepthCamSensorState depthState,
        //    double robotDepth, double robotAngle, CustomisedColor robotColor)
        public static Particle Propagate(List<Particle> particles, Parameter parameter, double jVelocity, List<Landmark> mapLandmarks, 
           double depth, double angle, CustomisedColor color, int level, out int lastindex)
        {
            if (particles == null)
            {
                lastindex = 0;
                return null;
            }

            Motions motion = parameter.Action;
            double iTime = parameter.Time;
            List<IdentifiedLandmark> identifiedLandmarks = parameter.IdentifiedLandmarks;
           
            if (level ==1)
            {

                foreach (Particle p in particles)
                {
                    foreach (Particle n in p.NestedParticles)
                        n.Weight = -1;
                    p.Weight = -1;
                }
            
            }

            int count = particles.Count;
            // for each particle
            //int i ;
            int lastjIndex = 0;
            ParallelOptions options = new ParallelOptions();
            options.MaxDegreeOfParallelism =4;
            Parallel.For(0, count, options, i =>
            {

                Particle particle = particles[i];
                RobotPose newPose;

                if (level == 1)
                {
                    newPose = SampleMotionModelVelocity(motion, particle.Pose, iTime);
                    if (!Particle.IsValidParticle(newPose))
                    {
                        Particle.PutParticlePoseInMap(newPose);
                        //newPose = particle.Pose;
                    }
                    particle.Pose = newPose;
                    //NOTE: for observation based approach set the if statement.
                    //if (particle.NestedParticles!= null && particle.NestedParticles[0].IsTagged)  
                    Propagate(particle.NestedParticles, parameter, jVelocity, mapLandmarks, depth, angle, color, level - 1, out lastjIndex);
                    Weight(null, particle, parameter, mapLandmarks, depth, angle, color, lastjIndex, level);
                }
                else
                {

                    bool careForCollision = true;
                    double timeForI = iTime;

                    double timeToPerformAction;
                    Motions jmotion;
                    if (particle.LeftoverTime > 0)
                    {
                        jmotion = particle.LeftoverMotion;
                        if (particle.LeftoverTime > iTime)
                            timeToPerformAction = iTime;
                        else
                        {
                            timeToPerformAction = particle.LeftoverTime;

                        }
                        careForCollision = false;
                    }

                    else
                    {
                        jmotion = GenerateMotionForJ(mapLandmarks, particle, JStraightProbability,
                                                     JRightProbability,
                                                     JLeftProbability, jVelocity, out timeToPerformAction);


                        if (particle.IsTagged && !particle.IsDone)
                            careForCollision = false;

                    }
                    if (particle.IsReached && !particle.IsDone)
                        jVelocity = JPushVelocity;

                    GeneratePoseForJ(particle, jmotion, iTime, timeToPerformAction, jVelocity, careForCollision);
                    timeForI -= timeToPerformAction;
                    while (timeForI > 0)
                    {
                        // we generate a motion for j and get a newtime to perform that action. if the newTime
                        // is negative, that means the robot is performing normal routine based on JAction time
                        jmotion = GenerateMotionForJ(mapLandmarks, particle, JStraightProbability,
                                                     JRightProbability,
                                                     JLeftProbability, jVelocity, out timeToPerformAction);
                        if (particle.IsTagged && !particle.IsDone)
                            careForCollision = false;
                        else
                            careForCollision = true;


                        GeneratePoseForJ(particle, jmotion, iTime, timeToPerformAction, jVelocity, careForCollision);
                        // if the time that takes for j to do an action is less than time that i takes to do the action
                        // since we are propagating based on i's action, that means j has more time to perform more than one action
                        timeForI = timeForI - timeToPerformAction; // left over time for j to do more action
                    }


                }//end of else
            });//end of for
            //lastindex = i;
            lastindex = count;
            Particle best = null;
                    
                if (level == 1)
                {
                    best = MostLikelyParticle(particles);
                    if(identifiedLandmarks != null && identifiedLandmarks.Count != 0)
                    {
                        Normalize(particles, 0, count);
                        //Debug.WriteLine("i: " + i);
                        Resample(particles, 0, count, 1);
                    }
                    

                }
            return best;
        }//end of method


        public static Particle MostLikelyParticle (List<Particle> particles)
        {
            Particle best = null;
            double maxWeight = -1; 
            foreach (Particle t in particles)
                if (t.Weight >= maxWeight)
                {
                    best = t;
                    maxWeight = t.Weight;
                }
            return best;
        }

        /// <summary>
        /// Recursive weighting procedure
        /// </summary>
        /// <param name="parentParticle">Parent Particle</param>
        /// <param name="particle">Particle</param>
        /// <param name="parameter">parameters</param>
        /// <param name="mapLandmarks">maplandmarks</param>
        /// <param name="landmarkdepth">depth of observed landmark</param>
        /// <param name="landmarkangle">angle of observed landmark</param>
        /// <param name="landmarkcolor">color of observed landmark</param>
        /// <param name="lastjIndex">last index of j in which we had time to propagate</param>
        /// <param name="level">level of nested particles</param>
        public static void Weight(Particle parentParticle, Particle particle, Parameter parameter, List<Landmark> mapLandmarks, 
            double landmarkdepth, double landmarkangle, CustomisedColor landmarkcolor, int lastjIndex, int level)
        {
            List<IdentifiedLandmark> identifiedLandmarks = parameter.IdentifiedLandmarks;
            DepthCamSensorState depthState = parameter.DepthState;
            bool observedRobot = parameter.IdentifiedRobot;
            double robotAngle = parameter.RobotAngle;
            double robotDepth = parameter.RobotDepth;
            CustomisedColor robotColor = parameter.RobotColor;

            if (level == 1)
            {
                if (identifiedLandmarks != null && identifiedLandmarks.Count != 0)
                {
                    ObserveLandmark(identifiedLandmarks, particle, landmarkdepth, landmarkangle);
                }
                else if ((identifiedLandmarks == null || identifiedLandmarks.Count == 0) && landmarkcolor != null)
                {
                    NegativeLandmarkObservation(particle, mapLandmarks, landmarkdepth, landmarkangle, landmarkcolor,
                                                depthState);
                }
            }
            else
            {
                if (observedRobot)
                {
                    ObserveRobot(parentParticle, robotDepth, robotAngle, robotColor);
                   
                }
                else
                {
                    NegativeRobotObservation(parentParticle, robotDepth, robotAngle, robotColor);
                   
                }
            }

            if (particle.NestedParticles != null )
            {
                for (int i = 0 ; i < particle.NestedParticles.Count; i++)
                {
                    Weight(particle, particle.NestedParticles[i] , parameter,mapLandmarks,landmarkdepth,landmarkangle,landmarkcolor,lastjIndex, level-1);
                }
           
                Normalize(particle.NestedParticles, 0, lastjIndex);
                Resample(particle.NestedParticles, 0, lastjIndex, 1);
            }
           
        }

        private static int counter = 0;
        /// <summary>
        /// This method generates a motion for j depending on its situation
        /// </summary>
        /// <param name="mapLandmarks">Real landmark positions</param>
        /// <param name="particle">particle of j</param>
        /// <param name="straightProb">straight probability</param>
        /// <param name="rightProb">turning right probability</param>
        /// <param name="leftProb">turning left probability</param>
        /// <param name="velocity">velocity m/sec</param>
        /// <param name="time">the time that it takes for j to perform the motion in seconds</param>
        /// <returns>Motion of j and the time it takes to perform the motion in second</returns>
        private static Motions GenerateMotionForJ (List<Landmark> mapLandmarks, Particle particle, 
            double straightProb, double rightProb, double leftProb, double velocity, out double time)
        {
            Debug.Assert(particle.LeftoverMotion == null,"leftover motion should be null before generating any motion for j dude!!!");
            Debug.Assert(particle.LeftoverTime ==0 ,"Left over time should be 0");
            Motions motion =  new Motions();
            time = 0;

            if (!(particle.IsTagged && !particle.IsTurned) && particle.LeftoverTime != 0)
            {
                return particle.LeftoverMotion;
            }
            if (!particle.IsTagged || particle.IsDone )//|| (particle.IsTurned))
            {
                try
                {
                    
                    motion = GenerateRandomMotion(straightProb, rightProb, leftProb,velocity);
                    time = JActionTime; 
                }
                catch (Exception e)
                {
                    Debug.WriteLine("Exception: "+e.ToString());
                }
            }// end of if !tagged || done
            else if (particle.IsTagged)
            {

               
                if (particle.LandmarkToPush == null)
                {
                    counter++;
                    Landmark closestLandmark = Landmark.ClosestLandmarkByDistance(mapLandmarks, particle.Pose);
                    particle.LandmarkToPush = (Landmark) closestLandmark.Clone();
                    particle.LandmarkIndex = (short) Landmark.FindIndexOfLandmark(mapLandmarks, closestLandmark);
                   
                }
               
                if (!particle.IsTurned)
                {
                    double theta = RobotPose.RotateTowardObject(particle.Pose, particle.LandmarkToPush.Center);
                    motion = new Motions();
                    velocity = JPushVelocity/1000;
                    motion.Velocity = velocity;
                    if (theta > 0)
                    {
                         motion.Omega = Drive.Omega(-velocity*1000, velocity*1000); // in degree/second 
                         time = Math.Abs(theta) / Math.Abs(motion.Omega); // second
                    }
                    else
                    {
                        motion.Omega = Drive.Omega(velocity*1000,-velocity*1000); // in degree/second 
                         time = Math.Abs(theta) / Math.Abs(motion.Omega); // second
                    }
                    particle.IsTurned = true;
                    particle.LeftoverMotion = null;
                    particle.LeftoverTime = 0;
                }
                else if (particle.IsTurned && !particle.IsReached)
                {
                    double theta = RobotPose.RotateTowardObject(particle.Pose, particle.LandmarkToPush.Center);
                    if (theta < -5 || theta > 5)
                    {
                        motion = new Motions();
                        velocity = JPushVelocity / 1000;
                        motion.Velocity = velocity;
                        if (theta > 0)
                        {
                            motion.Omega = Drive.Omega(-velocity * 1000, velocity * 1000); // in degree/second 
                            time = Math.Abs(theta) / Math.Abs(motion.Omega); // second
                        }
                        else
                        {
                            motion.Omega = Drive.Omega(velocity * 1000, -velocity * 1000); // in degree/second 
                            time = Math.Abs(theta) / Math.Abs(motion.Omega); // second
                        }
                        particle.IsTurned = true;
                        particle.LeftoverMotion = null;
                        particle.LeftoverTime = 0;

                    }
                    else
                    {
                        //Distance between particle and landmark.
                        double distance = Math.Sqrt(Math.Pow(particle.LandmarkToPush.Center.X - particle.Pose.X, 2) +
                                                    Math.Pow(particle.LandmarkToPush.Center.Z - particle.Pose.Z, 2));
                            // in meters

                        velocity = JPushVelocity/1000;
                        time = distance/velocity;
                        motion.Velocity = velocity;
                        motion.Omega = Drive.Omega(velocity, velocity);
                        // time = 5;
                        // Debug.WriteLine("Time to reach: " + time);
                        if (distance < 0.5)
                        {
                           // Debug.WriteLine("Reached");
                            particle.IsReached = true;
                           
                        }

                    }
                   
                }
                else if (particle.IsReached)
                {
                   
                    velocity = JPushVelocity / 1000;
                    motion.Velocity = velocity;
                    motion.Omega = Drive.StraightOmega;
                    time = 8;
                    particle.Pushed = true;

                }

                else
                {
                    Debug.Fail("It should not hit this location area 1" + particle.ToString());
                }
            }
            else
            {
                Debug.Fail("It should not hit this location area 2");
            }
            Debug.Assert(!(motion.Omega == 0 && motion.Velocity == 0), "Tagged:" + particle.IsTagged+
                "\nTurned: "+particle.IsTurned+"\nReached: "+ particle.IsReached+" \nPushed: "+ particle.Pushed+" \nDone: "+particle.IsDone);
           
            return motion;
        }


       /// <summary>
       /// Generates random motion for j when j is not tagged 
       /// </summary>
       /// <param name="straightProb"></param>
       /// <param name="rightProb"></param>
       /// <param name="leftProb"></param>
       /// <param name="velocity">Velocity in m/sec</param>
       /// <returns></returns>
       private static Motions GenerateRandomMotion (double straightProb, double rightProb, double leftProb, double velocity)
        {
            
            if (velocity < 0)
                throw new ArgumentOutOfRangeException("velocity","You need to set j's velocity value first (Part of static variable)");
            if ((straightProb + leftProb + rightProb) != 1)
                throw new ArgumentException(" Sum of all probabilities must be to one");
           
            Motions motion = new Motions();
 
            _rand = new Random(DateTime.Now.Millisecond*_count);

            _count++;
            double num = _rand.NextDouble();
           
            if (num <= straightProb)
            {
                motion.Velocity = velocity;
                motion.Omega = Drive.StraightOmega;
                
            }
            else if (num <= (straightProb + rightProb)) //rotate right
            {
                motion.Velocity = velocity;
                motion.Omega = Drive.Omega(velocity*1000,-velocity*1000); // in degree/second 
                
            }
            else
            {
                motion.Velocity = velocity;
                motion.Omega = Drive.Omega(-velocity*1000, velocity*1000); // in degree/second 
                
            }
           return motion;          
        }
     
        /// <summary>
        /// Generate Pose for nested particles of j, based on j's left over action or new action
        /// </summary>
        /// <param name="p">Single nested particle of j</param>
        /// <param name="motion"></param>
        /// <param name="iTime">The time i took to drive around and is used to propagate j;s particle at each 
        /// propagation step (in Seconds)</param>
        /// <returns></returns>
       private static void GeneratePoseForJ(Particle p, Motions motion, double iTime, double jTime,double jVelocity, bool careForCollision)
       {
           if (p == null)
               throw  new ArgumentNullException("p","particle cannot be null");
           if (motion == null)
               throw  new ArgumentNullException("motion","motion cannot be null");
           if (iTime <0)
               throw new ArgumentException("iTime","iTime cannot be negative");
           if (jTime <0 )
               throw  new ArgumentException("jTime","jTime cannot be negative");
           RobotPose newPose;

            

           bool iGreaterThanj = iTime >= jTime;

           if (iGreaterThanj)

               newPose = SampleMotionModelVelocity(motion, p.Pose, jTime);
           else
                newPose = SampleMotionModelVelocity(motion, p.Pose, iTime);

           if (careForCollision && !Particle.IsValidParticle(newPose))
           {
               motion = GenerateRandomMotion(0, 0.50, 0.50, jVelocity/1000);
               newPose = SampleMotionModelVelocity(motion, p.Pose, iTime); // generate a new pose
           }

            //TODO: comment out this for real run
               // newPose = jPose;
                //if (!Particle.IsValidParticle(newPose))
                //{
                //    Particle.PutParticlePoseInMap(newPose);
                //}
                if (Particle.IsValidParticle(newPose))
                    p.Pose = newPose;

                if (iGreaterThanj)
                {
                    
                    if (p.LeftoverTime -jTime < 0.01)
                    {
                        p.LeftoverTime = 0;
                        p.LeftoverMotion = null;
                    }
                    else
                    {
                        if (p.LeftoverTime > jTime)
                            p.LeftoverTime -= jTime;
                    }
                    if (p.Pushed && !p.IsDone&& !p.IsMapUpdated && p.LeftoverMotion == null && p.LeftoverTime == 0)
                    {
                        //Debug.WriteLine("updating pushed landmark in generatepose: p1");
                        p.IsDone = true;
                        if (!p.IsMapUpdated)
                        {
                            double xfactor = 0.0;
                            double zfactor = 0.0;
                            if (p.LandmarkToPush.Center.X < Map.XMinHallCorner || p.LandmarkToPush.Center.X > Map.XMaxHallCorner)
                                xfactor = 0.3;
                            else
                            {
                                zfactor = 0.3;
                            }
                            Point Center = new Point(p.Pose.X - xfactor, p.Pose.Z - zfactor);
                            if (!Particle.IsValidParticle(Center))
                            {
                                RobotPose pose = new RobotPose(Center.X, Center.Z, 0);
                                Particle.PutParticlePoseInMap(pose);
                                Center.X = pose.X;
                                Center.Z = pose.Z;
                            }
                            p.LandmarkToPush = new Landmark(p.LandmarkToPush.Name, Center,
                               p.LandmarkToPush.Length, p.LandmarkToPush.Width, p.LandmarkToPush.LandmarkColor);
                            p.LandmarkToPush.RecalculateCornerPoints();

                        }
                    }

                }
                else
                {
                    if (jTime - iTime > 0.0001)
                    {
                        p.LeftoverTime = jTime - iTime;
                        p.LeftoverMotion = motion;
                    }
                    else
                    {
                        p.LeftoverMotion = null;
                        p.LeftoverTime = 0;
                       
                    }
                   
                    if (p.Pushed && !p.IsDone && !p.IsMapUpdated && p.LeftoverMotion == null && p.LeftoverTime == 0)
                    {
                        //Debug.WriteLine("updating pushed landmark in generatepose: p2");
                        p.IsDone = true;
                        Debug.WriteLine("Done" + p.Pose);
                        if (!p.IsMapUpdated)
                        {
                            double xfactor = 0.0;
                            double zfactor = 0.0;
                            if (p.LandmarkToPush.Center.X < Map.XMinHallCorner || p.LandmarkToPush.Center.X > Map.XMaxHallCorner)
                            {
                                xfactor = 0.2;
                            }
                            else
                            {
                                zfactor = 0.2;
                            }
                            Point Center = new Point(p.Pose.X - xfactor, p.Pose.Z - zfactor);
                            if (!Particle.IsValidParticle(Center))
                            {
                                RobotPose pose = new RobotPose(Center.X, Center.Z, 0);
                                Particle.PutParticlePoseInMap(pose);
                                if (p.LandmarkToPush.Center.X < Map.XMinHallCorner || p.LandmarkToPush.Center.X > Map.XMaxHallCorner)
                                {
                                    Center.X = pose.X;
                                }
                                else
                                {
                                    Center.Z = pose.Z;
                                }
                               
                            }
                            p.LandmarkToPush = new Landmark(p.LandmarkToPush.Name, Center,
                               p.LandmarkToPush.Length, p.LandmarkToPush.Width, p.LandmarkToPush.LandmarkColor);
                            p.LandmarkToPush.RecalculateCornerPoints();

                        }
                    }
                      
                }
               
           
       }
         
              
           

      
        #endregion

        #region Observation

        /// <summary>
        /// Observation function, will weight the particles, normalize and resample them
        /// </summary>
        /// <param name="identifiedLandmarks">identified landmarks based on observation</param>
        /// <param name="particle">Particle</param>
        /// <param name="depth">depth in mm</param>
        /// <param name="angle">angle in degree</param>
        /// <exception cref="ArgumentNullException"><paramref name="identifiedLandmarks" /> is <c>null</c>.</exception>
        public static void ObserveLandmark(List<IdentifiedLandmark> identifiedLandmarks, Particle particle, double depth, double angle)
       {
           if (identifiedLandmarks == null)
               throw new ArgumentNullException("identifiedLandmarks", "Parameter cannot be null");
           if (particle == null)
               throw new ArgumentNullException("particle", "Particle shall not be null");
           Debug.Assert((identifiedLandmarks.Count != 0), "List is empty");

           CustomisedColor observedColor = identifiedLandmarks[0].ObservedColor;

           // based on the observed depth and angle, find out the location of the observed object
           Point point = GetGlobalPosition(particle.Pose, depth, angle);
           var observedEntity = new Entity(string.Empty, point, observedColor);

           IdentifiedLandmark facedLandmark = Landmark.ClosestLandmarkByAngle(identifiedLandmarks, particle.Pose);
           var mapEntity = new Entity(string.Empty, facedLandmark.Landmark.Center, facedLandmark.Landmark.EntityColor);
           double weight = ObservationProbability(observedEntity, particle.Pose, mapEntity);
           particle.Weight = weight;


       }

        /// <summary>
        /// Negative landmark observation will get rid of the particles that are close to the landmark but not consistant to the observation
        /// made by robot's sensor
        /// </summary>
        /// <param name="particle">particle of i</param>
        /// <param name="landmarks">list of believed landmarks</param>
        /// <param name="depth">depth in mm</param>
        /// <param name="angle">angle in degree</param>
        /// <param name="color">observed color</param>
        /// <exception cref="ArgumentNullException"><paramref name="particle" /> is <c>null</c>.</exception>
        public static void NegativeLandmarkObservation(Particle particle, List<Landmark> landmarks, double depth, double angle, CustomisedColor color, DepthCamSensorState depthState)
       {
           if (landmarks == null)
               throw new ArgumentNullException("landmarks", "Parameter cannot be null");
           if (particle == null)
               throw new ArgumentNullException("particle", "Particle shall not be null");

           // find the closest landmark if any to the particle based on the threshold.
           Landmark closestLm = Landmark.ClosestLandmarkByThreshold(particle.Pose, landmarks, DepthAnalysis.FieldOfView,
               LowNegativeObsThreshold, UpNegativeObsThreshold);
           // if there was any close landmark to the praticle... give that particle a low weight
           if (closestLm != null)
           {
               //since no landmark was observed, give a center as far as possible to the robot's location (particle in this case)
               Point center = GetGlobalPosition(particle.Pose, 3000, 0);
               var observedFeatures = new Landmark("landmark", center, 0.5, 0.5, color); // this is just what the robot has observed. put inside the landmark object. 

               double prob = ObservationProbability(observedFeatures, particle.Pose, closestLm);
               particle.Weight = prob;
           }
           if (closestLm == null)
           {
               Point center = GetGlobalPosition(particle.Pose, 3000, 0);
               var observedFeatures = new Landmark("landmark", center, 0.5, 0.5, color); // this is just what the robot has observed. put inside the landmark object. 
               closestLm = new Landmark("landmark", center, 0.5, 0.5, color); // just like observed features
               double prob = ObservationProbability(observedFeatures, particle.Pose, closestLm);
               particle.Weight = prob;
           }
       }

        /// <summary>
        /// Positive opponant observation. It will weigh the nested particles 
        /// </summary>
        /// <param name="particles">particle of i</param>
        /// <param name="depth">depth in mm</param>
        /// <param name="angle">angle in degree</param>
        /// <param name="observedColor">observed color of the robot</param>
        public static void ObserveRobot(Particle particle, double depth, double angle, CustomisedColor observedColor)
        {

            // Particle particle = particles[i];
            Point point = GetGlobalPosition(particle.Pose, depth, angle);
            var observedJLocation = new Entity(string.Empty, point, observedColor);
            Parallel.For(0, particle.NestedParticles.Count, j =>
                                                                {

                                                                    Particle jParticle = particle.NestedParticles[j];
                                                                    // where i believes j is
                                                                    var particleJ = new Entity(string.Empty,
                                                                                               new Point(
                                                                                                   jParticle.Pose.X,
                                                                                                   jParticle.Pose.Z),
                                                                                               observedColor);

                                                                    double weight = ObservationProbability(particleJ,
                                                                                                           particle.Pose,
                                                                                                           observedJLocation);
                                                                    particle.NestedParticles[j].Weight = weight;
                                                                });
            // At what level should we normalize?... at level of i or j?? or over all j's particle?
        }

        public static void NegativeRobotObservation(Particle particle, double depth, double angle, CustomisedColor observedColor)
        {

            Point observeObjectPose = GetGlobalPosition(particle.Pose, depth, angle);
            var observeObject = new Entity("", observeObjectPose, observedColor);
            // for every particle j of i
            // find the closest opponent particles if any to the particle i based on the threshold.
            Parallel.For(0, particle.NestedParticles.Count, j =>
                                                                {
                                                                    Particle nested = particle.NestedParticles[j];
                                                                    double theta = Math.Atan2
                                                                        (-nested.Pose.X + particle.Pose.X,
                                                                         -nested.Pose.Z + particle.Pose.Z);
                                                                    theta = UnitConversion.RadianToDegree(theta);
                                                                    theta = Math.Abs(theta - particle.Pose.Theta);
                                                                    double distanceBtwPoses =
                                                                        Math.Sqrt(
                                                                            Math.Pow(nested.Pose.X - particle.Pose.X, 2) +
                                                                            Math.Pow(nested.Pose.Z - particle.Pose.Z, 2));

                                                                    if (theta > 0 && theta < DepthAnalysis.FieldOfView &&
                                                                        distanceBtwPoses < 5)
                                                                    {
                                                                        var jParticle = new Entity("",
                                                                                                   new Point(
                                                                                                       nested.Pose.X,
                                                                                                       nested.Pose.Z),
                                                                                                   observedColor);
                                                                        double prob = ObservationProbability(jParticle,
                                                                                                             particle.
                                                                                                                 Pose,
                                                                                                             observeObject);
                                                                        nested.Weight = prob;
                                                                    }
                                                                    else
                                                                    {
                                                                        Point pose =
                                                                            new Point(observeObject.Center.X + 0.2,
                                                                                      observeObject.Center.Z + 0.2);
                                                                        var jParticle = new Entity("", pose,
                                                                                                   observedColor);
                                                                        double prob = ObservationProbability(jParticle,
                                                                                                             particle.
                                                                                                                 Pose,
                                                                                                             observeObject);
                                                                        particle.NestedParticles[j].Weight = prob;
                                                                    }

                                                                });
        }


        /// <summary>
        /// Gets global postion of the observed object based on the position of the particle/robot
        /// </summary>
        /// <param name="pose">pose of the robot/particle</param>
        /// <param name="depth">depth in mm</param>
        /// <param name="angle">angle in degree</param>
        /// <returns>global position of the observed object</returns>
        /// <exception cref="ArgumentNullException"><paramref name="pose" /> is <c>null</c>.</exception>
        private static Point GetGlobalPosition(RobotPose pose, double depth, double angle) // ******make it private
        {
            if (pose == null)
                throw new ArgumentNullException("pose", "Robotpose may not be null");
            Point point = new Point();
            depth /= 1000.0; // in mm
            if (depth == 0)
            {
                point.X = 0;
                point.Z = 0;
                return point;
            }
            angle += pose.Theta; // in degree    
            angle = UnitConversion.DegreeToRadian(angle); // in radian    
            angle = UnitConversion.PutThetaInBound(angle);
            double x = -(depth * Math.Sin(angle)) + pose.X;
            double z = -(depth * Math.Cos(angle)) + pose.Z;

            point.Z = z;
            point.X = x;
            return point;
        }

        public static double GetAngle (Entity e , RobotPose p)
        {
            double theta = Math.Atan2(-e.Center.X + p.X, -e.Center.Z + p.Z);   // in radian
            theta = UnitConversion.PutThetaInBound(theta);
            if (p.Theta >= 0)
                theta = theta - UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(p.Theta));
            else
                theta = UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(p.Theta)) - theta;
            theta = UnitConversion.PutThetaInBound(theta);
            theta = UnitConversion.RadianToDegree(theta);
            return theta;
        }

        #endregion

        #region Observation Probability

        /// <summary>
        /// This will calculate the probability of a particle being 
        /// </summary>
        /// <param name="observedLandmark">Observed landmark</param>
        /// <param name="particlePose">Particle's pose</param>
        /// <param name="mapLandmark">Landmark in the map</param>
        /// <returns>Probability of the partcile </returns>
       
        private static double ObservationProbability(Entity observedLandmark, RobotPose particlePose, Entity mapLandmark) //** make it private
        {
            // TO DO

            double s = ((mapLandmark.EntityColor.Hue - observedLandmark.EntityColor.Hue) +
                (mapLandmark.EntityColor.Saturation - observedLandmark.EntityColor.Saturation) + 
                (mapLandmark.EntityColor.Luminance - observedLandmark.EntityColor.Luminance)) / 3;
            
            double rhat = Math.Sqrt(Math.Pow(mapLandmark.Center.Z - particlePose.Z, 2) + Math.Pow(mapLandmark.Center.X - particlePose.X, 2));           // real distance of the object from the robot/particle.
            // Debug.WriteLine("\tDistance of the map object from the particle (rhat) : " + rhat);
            /* Console.WriteLine ("\tLandmark XCenter: "+m.Center.X +"\n\tParticle X:"+pose.x);
             Console.WriteLine("\tLandmark ZCenter: " + m.Center.Z + "\n\tParticle Z:" + pose.z);
             Console.WriteLine ("\tTheta of the particle: "+pose.theta);
             Console.WriteLine("\tArctan of the landmark and particle: " + Math.Atan2(m.Center.X - pose.x, m.Center.Z - pose.z));
            
             */
            double fihat = Math.Atan2(-mapLandmark.Center.X + particlePose.X, -mapLandmark.Center.Z + particlePose.Z);   // in radian
            // Console.WriteLine("y:" + (-m.Center.X + pose.x) + " x: " + (-m.Center.Z + pose.z));
            // Console.WriteLine("fihat before putting in bound : " + MathHelper.RadianToDegree(fihat));
            fihat = UnitConversion.PutThetaInBound(fihat);
            if (particlePose.Theta >= 0)
                fihat = fihat - UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(particlePose.Theta));
            else
                fihat = UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(particlePose.Theta)) - fihat;
            fihat = UnitConversion.PutThetaInBound(fihat);

            //fihat = Math.Abs(fihat);
            //Debug.WriteLine("\tThe theta between map landmark and particle: " + UnitConversion.RadianToDegree(fihat));
            double fi = Math.Atan2(-observedLandmark.Center.X + particlePose.X, -observedLandmark.Center.Z + particlePose.Z);   // in radian
            //Console.WriteLine("y:" + (-observedObject.Center.X + pose.x) + " x: " + (-observedObject.Center.Z + pose.z));
            //Console.WriteLine("fi before putting in bound : " + MathHelper.RadianToDegree(fi));
            fi = UnitConversion.PutThetaInBound(fi);
            if (particlePose.Theta >= 0)
                fi = fi - UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(particlePose.Theta));
            else
                fi = UnitConversion.PutThetaInBound(UnitConversion.DegreeToRadian(particlePose.Theta)) - fi;
            fi = UnitConversion.PutThetaInBound(fi);
            //fi = Math.Abs(fi);
            // Debug.WriteLine("\ttheta between observedLandmark and particle : " + UnitConversion.RadianToDegree(fi));
            double r = Math.Sqrt(Math.Pow(observedLandmark.Center.Z - particlePose.Z, 2) + Math.Pow(observedLandmark.Center.X - particlePose.X, 2)); // distance of the observed object from the robot
            //Console.WriteLine("R:" + r);
            
           // Debug.WriteLine("\tDistance of the observed object from the particle : " + r);
            /*Console.WriteLine("\t r-rhat : " + (r - rhat) + " p: " + Probability.prob_normal_distribution(r - rhat, 0, varR));
            Console.WriteLine("\t fi-fhat: " + (fi - fihat) + " p: " + Probability.prob_normal_distribution(fi - fihat, 0, varFi));
            Console.WriteLine("\ts  : " + (s) + " p: " + Probability.prob_normal_distribution(s, 0, varS));
            */
            double q = Probability.ProbNormalDistribution(r - rhat, 0, VarR) * 
                Probability.ProbNormalDistribution(UnitConversion.PutThetaInBound(fi - fihat), 0, VarFi) * 
                Probability.ProbNormalDistribution(s, 0, VarS);
            //Console.WriteLine("\tProbability is : " + q);
            
            Debug.Assert(!Double.IsNaN(q) , "We cannot have a NAN value for weight" ); 
            //Debug.Assert(q!= 0, "We cannto have a 0 weight.");
            return q;
        }

        #endregion

        #region Resample
        /// <summary>
        /// After the weights have been normalized, the particles are resampled based
        /// on their weights value. The higher the weights, the higher is the probability of
        /// a particle to be selected
        /// </summary>
        /// <param name="particles"> list of particles</param>
        private static void Resample ( List<Particle> particles, int startIndex, int endIndex, double percentage)
        {
            //percentage to be resampled from the propagated area.
            Debug.Assert(percentage <= 1 , "Percentage should be less than one");
            Debug.Assert(particles!= null, "Particle cannot be null");
            Debug.Assert(startIndex <= endIndex, "Start index (" + startIndex+") must be less than endindex ("+endIndex+")");
            Debug.Assert(endIndex <= particles.Count, "end index must be less than particles count");
            Debug.Assert(particles.Count != 0 ,"list cannot be empty. Start index:"+ startIndex + " end index: " + endIndex );
            int numOfPropagated = endIndex - startIndex; // number of propagated particles
            if (numOfPropagated == 0)
                return;
            
            
            double[] weights = new double[numOfPropagated]; // contains the accumulative sum of all the weights
            int totalNumOfParticles = particles.Count;
            
            //if (numOfPropagated == 0)
            //{
            //    for (int i = 0; i < totalNumOfParticles; i++)
            //    {
            //        particles[i].Weight = -1;
            //    }
            //    return;

            //}


            int count = 0;
            double sum = 0.0;

            for (int i = startIndex; i < endIndex; i++ )
            {
                weights[count] = sum + particles[i].Weight;
                sum += particles[i].Weight;
                count++;
            }

           
            Debug.Assert(count == numOfPropagated,"weights' length should be equal to count");

            // num of particles to be sampled from the propagated portion
            int c1 = (int)((totalNumOfParticles) * percentage); // the number of particles that must be sampled from the propagated portion
            
            //Debug.WriteLine("C1: " + c1 + " total num of particles: " + totalNumOfParticles);
            
            for (int i  = 0 ; i < c1 ; i++ ) //resample n times( n = number of particles)
            {
                _rand = new Random(DateTime.Now.Millisecond * i);
                double randomNumber = _rand.NextDouble();
                int j;
                for ( j = 0 ; j < weights.Length; j++)
                {
                    if (randomNumber <= weights[j] )
                    {
                        particles.Add( (Particle) particles[j].Clone());
                        //particles[j].Weight = -1;
                        break; //breaks out of the inner loop
                    }
                }
               
                Debug.Assert(j!= weights.Length," couldnt pick any number in the weighting section... " + j + " " + weights.Length + " num of propagated: "+ numOfPropagated);
            }
          
            // num of particles to be sampled from the non propagated portion
            int c2 = totalNumOfParticles - c1;
            //Debug.WriteLine("C2: "+ c2);
            if (c2 != 0)
                Debug.WriteLine("C2: " + c2 + " total particles: "+ totalNumOfParticles + " stindex: "+ startIndex + " endindex: "+ endIndex );
            for (int i = 0; i < c2; i++) 
            {
                _rand = new Random(DateTime.Now.Millisecond * i);
                // generate a random number between last index and particles.count
                int index = _rand.Next(endIndex, particles.Count);
                particles.Add((Particle)particles[index].Clone());
                //particles[index].Weight = -1;
            }
            //Debug.WriteLine("particles.count: "+ particles.Count);
            particles.RemoveRange(0, totalNumOfParticles);
            Debug.Assert(particles.Count != 0, "list cannot be empty after resampling" );
            Debug.Assert(particles.Count == totalNumOfParticles, "Particles should contain" + totalNumOfParticles + "particles. Particles count: " + particles.Count);
        }
        #endregion

        #region Normalize
        /// <summary>
        /// normalizes all the particles's weight, putting them in the range of [0,1]
        /// </summary>
        /// <param name="particles">list of particles</param>
        private static void Normalize (List<Particle> particles, int startIndex, int endIndex)
        {
            Debug.Assert(particles!= null,"Particles cannot be null");
            Debug.Assert(particles.Count!= 0, "List shall not be empty. StartIndex: "+ startIndex + "EndindeX:" +endIndex);
            Debug.Assert(startIndex <= endIndex, "Start index (" + startIndex + ") must be less than endindex (" + endIndex + ")");
            Debug.Assert(endIndex <= particles.Count, "end index must be less than particles count");
            
           
            double sum = 0;//particles.Sum(particle => particle.Weight);
            for (int i = startIndex; i < endIndex; i++ )
            {
                Debug.Assert(particles[i].Weight != -1, "Weight should be positive " + particles[i].Weight + " startIndex: "+ startIndex + " endIndex:"+ endIndex + " index: "+ i );
                Debug.Assert(!Double.IsNaN(particles[i].Weight),"weight should be a number...");
                sum += particles[i].Weight;
            }
             
            
            for (int i = startIndex; i < endIndex; i++)
            {
                if (sum==0 )
                {
                    double denominator;
                    if (startIndex - endIndex == 0)
                        denominator = particles.Count;
                    else
                        denominator = endIndex - startIndex;
                    
                    particles[i].Weight = 1.0 / denominator;
                }
                    
                else
                {
                    particles[i].Weight = (particles[i].Weight) / sum;
                }
               
                
            }
            for (int i = endIndex; i < particles.Count; i++)
                particles[i].Weight = -1;
                Debug.Assert(particles.Count != 0, "List shall not be empty after normalization. StartIndex: " + startIndex + "EndindeX:" + endIndex);
        }

        public static int CompareWeight(Particle p1, Particle p2)
        {
            if (p1.Weight > p2.Weight)
                return -1;
            if (p1.Weight == p2.Weight)
                return 0;
            return 1;
        }

        #endregion
    }

  

    #region Probability Class
    /// <summary>
    /// Probability functions
    /// </summary>
    public class Probability
    {

        /// <summary>
        /// This function calculates the probability normal distribution/ Guassian distribution
        /// </summary>
        /// <param name="a">variable</param>
        /// <param name="mean">mean/average</param>
        /// <param name="var">Variance (sigma * sigma) </param>
        /// <returns>Probability normal distribution</returns>
        public static double ProbNormalDistribution(double a, double mean, double var)
        {
            //Console.WriteLine("a : " + a + " var: " + var);
            double temp = 1 / (Math.Sqrt(2 * Math.PI * var));
            double power = Math.Pow(a - mean, 2) / (2 * var);
            double y = Math.Exp(-power);
            //Gaussian g = new Gaussian(Math.Sqrt(var));
            //Debug.WriteLine("Gaussian: "+ g.Function(a-mean));
            //Console.WriteLine("./................... : " + temp * y);))
            return temp * y;
        }

        /// <summary>
        ///  Normal Model Distribution with mean 0 and variance b^2
        /// </summary>
        /// <param name="var">Variance</param>
        /// <returns></returns>
        public static double SampleNormalDistribution(double var)
        {
            //Console.WriteLine("************* Probability************");
            // Console.WriteLine("var: " + var);
            double vpos = Math.Sqrt(var);
            double vneg = -Math.Sqrt(var);
            double x;

            // Console.WriteLine("Upper and lower bound: " + bpos + ", " + bneg);
            double sum = 0.0;
            Random rand = new Random(DateTime.Now.Millisecond);
            for (int i = 0; i < 12; i++)
            {
                x = vneg + rand.NextDouble() * (vpos - vneg);
                //x = rand.Next(vneg, vpos);
                // Console.Write( x+ " ");
                sum += x;
            }
            //Console.WriteLine("answer: " + sum*0.5);
            return 0.5 * sum;

        }

    }
    #endregion 
}
